﻿// PointFilter.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#define _CRT_SECURE_NO_WARNINGS 1
#define PI 3.1415926535
#include "Point.h"
#include "KDTree.h"


float disThre = 0.0;
int minPoints = 10;
int vecUnitSize = 0;

std::vector<bool> isVisited; 

//基于密度可达性质，记录邻域点簇，输出时全部加入
std::vector<std::vector<int>> clusters;

std::vector<std::vector<int>> neighbor;

std::map<std::string, float>* disPts = new std::map<std::string, float>();


bool compareX(Point& a, Point& b)
{
    return a.x >= b.x;
}
bool compareY(Point& a, Point& b)
{
    return a.y >= b.y;
}
bool compareZ(Point& a, Point& b)
{
    return a.z >= b.z;
}
bool cmpVector(std::vector<int>& a, std::vector<int>& b)
{
    return a.size() > b.size();
}
//计算局部离群因子
inline bool LOF(int ptIndex,int cluster, std::map<std::string, float>* distanceCollection) {
    
    //计算添加此点之前的平均密度
    //std::vector<int> p = neighborPt[cluster];
    //要加入的簇
    auto p = neighbor[cluster];
    if (p.size() <= 0) return false;
    float dis1 = 0.0;
    float dis2 = 0.0;
    float den1 = 0.0;
    float den2 = 0.0;
    for (int i = 0; i < p.size(); i++)
    {
        //Point p1 = getPointFromMap(p[i]);
        for (int j = 0; j < p.size(); j++)
        {
            //Point p2 = getPointFromMap(p[j]);
            std::string d = std::to_string(i) + ":" + std::to_string(j);
            dis1 += (*distanceCollection)[d];
        }
    }
    den1 = dis1 / p.size();
    //添加之后的平均密度
    dis2 = dis1;
    for (int k = 0; k < p.size(); k++)
    {
        //Point p3 = getPointFromMap(p[k]);
        std::string d1 = std::to_string(ptIndex) + ":" + std::to_string(p[k]);
        //dis2 += caleuDistance(pt, p3);
        dis2 += (*distanceCollection)[d1];
    }
    den2 = dis2 / (p.size() + 1);
    
    return (den2 / den1) >= 1.2;
}

//初始化
inline void init(int index)
{
    isVisited.push_back(false);
    clusters.push_back(std::vector<int>());
    //coreNeighborPt.push_back(std::vector<int>());
    //neighborPt.push_back(std::vector<int>());
    neighbor.push_back(std::vector<int>());
    
}

//合并类簇
void combineCluster(int index,int length,std::vector<std::vector<int>> sum)
{
    for (int j = 0; j < sum.size(); j++)
    {
        if (j == index) continue;
        //sum->push_back(j);
        combineCluster(j, length, sum);
    }
}

inline void DBSCAN(KDTree* tree,int length,float disThre,float minPts, std::map<std::string, float>* distanceCollection)
{
    //检测
    for (int i = 0; i < length; ++i) {
        /*
        int currper = std::ceil((float)i / (float)length);
        {
            system("cls");
            std::cout << "当前进度：" << currper << " %," << "正在处理第 " << i << " 个" << std::endl;
        }
        */
        if (!isVisited[i]) {
            isVisited[i] = true;

            auto clusterI = clusters[i];
            clusterI.push_back(i);

            //auto neigh = neighbor[i];

            //构建KDTree
            std::vector<int> ptCollection;
            auto currPt = tree->getParent()->data[i];
            KDNode* node = tree->getLeafNode(tree->getParent(),currPt);
            //tree->constructKDTree();
            tree->searchKNode(ptCollection, node, disThre, currPt);

            //std::cout << "找到点：" << ptCollection.size() << ",树高:" << tree->getTreeHeight() << std::endl;

            for (int j = 0; j< ptCollection.size();j++)
            {

                if (j == i || isVisited[j]) continue;
                std::string findPt = std::to_string(i) + ":" + std::to_string(j);
                float dis = (*distanceCollection)[findPt];

                if (dis < disThre && !LOF(j, i, distanceCollection))
                {
                    //如果已经访问且可达，则按被访问ClusterId设置
                    
                    clusters[i].push_back(j);
                    neighbor[i].push_back(j);
                    //neigh.emplace_back(j);
                    //if((*clusterId)[j] == j) (*clusterId)[j] = i;
                }
            }
        }
    }
    //合并
    for (int k = 0; k < length; k++)
    {
        auto c = clusters[k];
        if (clusters[k].empty()) continue;
        for (auto i : c)
        {
            if (clusters[i].empty()) continue;
            std::copy(clusters[i].begin(), clusters[i].end(), std::back_inserter(c));
            clusters[i].clear();
        }
    }
    std::sort(clusters.begin(), clusters.end(), cmpVector);
}

//体素化
void Voxelization()
{

}

//计算点间距离
void getPtDis(std::vector<Point>* src, std::map<std::string, float> *dst)
{
    int n = 0;
    
    std::string store;
    //int storeI = 0;
    int storeJ = 0;
    for (int i = 0; i < src->size(); i++)
    {
        //printf("i:%d,\n", i);
        Point pt1 = (*src)[i];
        store = std::to_string(i) + ":" + std::to_string(storeJ + 1);
        for (int j = i + 1; j < src->size(); j++)
        {
            storeJ = j;
            if (i == j) continue;
            Point pt2 = (*src)[j];
            dst->insert(std::make_pair(store, caleuDistance(pt1,pt2)));
            //printf("dis:%f,i:%d,j:%d\n", caleuDistance((*src)[i], (*src)[j]),i,j);
        }
    }
}
//计算高斯分布参数
std::pair<float, float> getGussianDistribution(std::map<std::string, float>* dis)
{
    float miu;
    float sigma;

    float sum = 0.0;
    float sum_squares = 0.0;
    float n = dis->size();
    for (auto data : *dis)
    {
        sum += data.second;
        sum_squares += data.second * data.second;
    }
    miu = sum / n;
    sigma = std::sqrt(sum_squares / n - miu * miu);
    std::pair<float, float> res = std::make_pair(miu, sigma);
    return res;

}
//高斯分布函数
inline float gaussian(float x, float mean, float std_dev)
{
    return (1.0 / (std::sqrt(2.0 * PI) * std_dev)) * std::exp(-(x - mean) * (x - mean) / (2.0 * std_dev * std_dev));
}

//获取文件行数
inline int getRows(std::ifstream& file)
{
    int line = 0;
    std::string str;
    while (std::getline(file, str))
    {
        ++line;
    }
    return line;
}

int readFile(const char* srcFile, std::vector<Point> &pts,std::vector<std::vector<int>> neighbor)
{
    char temp[60];
    std::string str;
    std::ifstream ifs(srcFile);

    int length = getRows(ifs);
    int index = 0;
    pts.reserve(length);
    neighbor.reserve(length);
    if (length > 0)
    {
        //清除eof标志位
        ifs.clear();
        ifs.seekg(0, std::ios::beg);

        while (std::getline(ifs, str))
        {
            Point pt;
            //std::cout << str.c_str() << std::endl;
            memset(temp, '\0', sizeof(temp));
            strcpy_s(temp, sizeof(temp), str.c_str());

            //strtok 分割字符串
            char* x = strtok(temp, " ");
            char* y = strtok(NULL, " ");
            char* z = strtok(NULL, " ");

            if (x != NULL && y != NULL && z != NULL)
            {
                pt.x = atof(x);
                pt.y = atof(y);
                pt.z = atof(z);
            }
            pt.cluster = index;
            pts.push_back(pt);
            init(index);
           // clusterId.push_back(index);
            ++index;
        }

    }
    ifs.close();
    return length;
}

void outputFile(std::vector<Point> *srcPt, const char* dstFile, std::vector<std::vector<int>>& outputPts)
{
    int count = 0;
    //for (auto pts = outputPts.begin(); pts != outputPts.end(); pts++)
    for (auto pt = 0; pt < outputPts.size() / 10; pt++)
    {
        auto &pts = outputPts[pt];

        if (pts.size() < minPoints) continue;
        FILE* ptsFile = fopen(dstFile + static_cast<char>(count) + '.pts', "w");
        for (auto tetra = pts.begin(); tetra != pts.end(); tetra++)
            //for (std::list<Triangle*>::iterator tetra = delaunyTraList.begin(); tetra != delaunyTraList.end(); tetra++)
        {
           
            Point resPt = (*srcPt)[(*tetra)];
            std::string str1;
            str1 = std::to_string(resPt.x) + " " + std::to_string(resPt.y) + " " + std::to_string(resPt.z) + "\n";
            fprintf(ptsFile, str1.c_str());
            //printf("%s\n", str1.c_str());
        }
        printf("--------------------------------\n");
        ++count;
        fclose(ptsFile);
    }


    
}

int main()
{
    //std::string srcFile = "E:\\项目\\噪点去除\\半个房间\\半个房间\\no-1.pts";
    //std::string dstFile = "E:\\项目\\噪点去除\\半个房间\\半个房间\\test";
    std::string srcFile = "D:\\Visual Studio Project\\PointFilter\\半个房间\\半个房间\\no-1.pts";
    std::string dstFile = "D:\\Visual Studio Project\\PointFilter\\半个房间\\半个房间\\test";
    std::vector<std::vector<Point>> output;
    std::vector<Point> pts; 
    std::vector<nearPoints> nearPts;
    std::vector<int> clusterId;

    printf("正在加载文件...\n");
    int length = readFile(srcFile.c_str(), pts, neighbor);
    printf("1.正在构建KDTree...\n");
    KDTree* tree = new KDTree(pts);
    tree->constructKDTree();
    printf("KDTree构建完成\n");
    printf("2.正在计算点云间距离...\n");
    getPtDis(&pts, disPts);
    printf("点云间距离计算完成\n");
    printf("3.正在统计距离、拟合高斯模型、计算高斯参数...\n");
    //获取高斯参数
    std::pair<float, float> gaussianParam = getGussianDistribution(disPts);
    
    //r = |μ-2σ|
    float disThre = std::abs(gaussianParam.first - (2 * gaussianParam.second));
    printf("高斯参数计算完成\n");
    printf("4.正在使用DBScan结合KDTree搜索...\n");
    //检测
    DBSCAN(tree,length,disThre,minPoints,disPts);
    printf("搜索完成\n");
    //输出
    printf("4.正在输出文件...\n");
    outputFile(&pts,dstFile.c_str(), clusters);
    printf("输出完成\n");
 

    
    system("pause");
}

// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
// 调试程序: F5 或调试 >“开始调试”菜单

// 入门使用技巧: 
//   1. 使用解决方案资源管理器窗口添加/管理文件
//   2. 使用团队资源管理器窗口连接到源代码管理
//   3. 使用输出窗口查看生成输出和其他消息
//   4. 使用错误列表窗口查看错误
//   5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
//   6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
